In [1]:
from shapely.geometry import LineString, Point
from shapely.ops import transform
from shapely import affinity
from shapely.ops import cascaded_union
from math import sqrt
from functools import partial
In [2]:
import pyproj
utm32 = pyproj.Proj(init='epsg:32632')
wgs84 = pyproj.Proj(init='epsg:4326')

def wgs84tomap_transform(map_origin, map_orientation=0.0):
    x0, y0 = pyproj.transform(wgs84, utm32, *map_origin)
    def t(y, x):
        x1, y1 = pyproj.transform(wgs84, utm32, x, y)
        x1 = x1 - x0
        y1 = y1 - y0
        if map_orientation:
            x1 = cos(map_orientation)*x1 + sin(map_orientation)*y1
            y1 = - sin(map_orientation)*x1 + cos(map_orientation)*y1
        return (x1, y1)
    return t
In [3]:
o_img = (8.91909, 46.02620) 
o_map = (8.919781, 46.025830)
project = wgs84tomap_transform(o_map)
o_utm32_img = pyproj.transform(wgs84, utm32, *o_img)
o_utm32_map = pyproj.transform(wgs84, utm32, *o_map)
mx = o_utm32_img[0] - o_utm32_map[0]
my = o_utm32_img[1] - o_utm32_map[1]
In [4]:
from owslib.wms import WebMapService
import wms_credentials
import PIL.Image
import io

wms_url = "https://wms.swisstopo.admin.ch/wss/httpauth/swisstopowms/"
wms = WebMapService(wms_url, username=wms_credentials.username, password=wms_credentials.password)
layer_name = 'ch.swisstopo.swisstlm3d-karte-grau'
image_format = 'image/jpeg'
crs = 'EPSG:32632' #WGS 84 / UTM zone 32N
assert layer_name in wms.contents
assert image_format in wms.getOperationByName('GetMap').formatOptions

layer = wms[layer_name]
assert crs in layer.crsOptions

def get_map(center, size, px_width=640):
    w, h = size
    x, y = center
    bbox = (x-w*0.5, y-h*0.5, x+w*0.5, y+h*0.5)
    px_size = (px_width, int(px_width*h/w))
    img = wms.getmap(layers=[layer_name], srs=crs, bbox=bbox, format=image_format, transparent=True, size=px_size)
    m = PIL.Image.open(io.BytesIO(img.read()))
    m.origin = center
    m.o_size = size
    return m
In [5]:
def plot_map(map_img,  map_origin, size=None,):
    if size is None:
        size = map_img.o_size
    w, h = size
    x, y = map_img.origin
    ox, oy = map_origin
    return imshow(map_img, extent=(x-ox-w/2, x-ox+w/2, y-oy-h/2, y-oy+h/2))
In [6]:
i = get_map(o_utm32_img, (300, 300), px_width=800)
i
Out[6]:
In [7]:
import numpy as np
In [8]:
import rosbag
from sensor_msgs.msg import Imu, NavSatFix
from nav_msgs.msg import Odometry
import std_msgs.msg
In [9]:
from matplotlib.pyplot import plot, axis, figure, imshow, legend, polar
import matplotlib.image as mpimg
%matplotlib inline
import mpld3
mpld3.enable_notebook()
import descartes
In [10]:
def plot_cov(ax, p, **kwargs):
    if p:
        patch = descartes.PolygonPatch(p, alpha=0.5, zorder=1, **kwargs)
        return ax.add_patch(patch)
    return None
In [11]:
def plot_path(ax, line, **kwargs):
    if line:
        x,y = line.xy
        return ax.plot(x, y,'.-', linewidth=1, solid_capstyle='round', zorder=2, **kwargs)[0]
    else:
        return None
In [12]:
def cov_ellipse(center, cov):
    cov2d = cov[:2,:2]
    e, v = np.linalg.eig(cov2d)
    theta = np.arccos(v[0][0])
    p = Point(center).buffer(1)
    p = affinity.scale(p, sqrt(e[0]),sqrt(e[1]))
    p = affinity.rotate(p, theta)
    return p
In [13]:
def cov_ellipse_fix(d):
    cov = d.position_covariance
    p = Point(project(d.latitude, d.longitude)).buffer(1)
    return affinity.scale(p, sqrt(cov[0]),sqrt(cov[4]))
In [14]:
def odom_covariance(bag, topic):
    ms = bag.read_messages(topics=[topic])
    es = [cov_ellipse((d.pose.pose.position.x, d.pose.pose.position.y),np.array(d.pose.covariance).reshape(6,6)) 
          for i, (_, d, td) in enumerate(ms) if i % 50 == 0]
    if len(es)>1:
        return cascaded_union(es[1:-20])
    else:
        return None
In [15]:
def fix_covariance(bag, topic):
    ms = bag.read_messages(topics=[topic])
    es = [cov_ellipse(project(d.latitude, d.longitude),np.array(d.position_covariance).reshape(3,3)) for _, d, td in ms]
    if es:
        return cascaded_union(es)
    else:
        return None
In [16]:
def fix_path(bag, topic):
    ms = bag.read_messages(topics=[topic])
    poses = [(d.latitude, d.longitude) for _, d, ts in ms]
    if poses:
        return transform(project, LineString(poses))
    return None
In [17]:
def odometry_path(bag, topic):
    ms = bag.read_messages(topics=[topic])
    poses = [d.pose.pose.position for _, d, ts in ms]
    if poses:
        return LineString([(p.x,p.y) for p in poses])
    return None
In [18]:
def plot_paths(bag):
    fig = figure(figsize=(12,12))
    ax = fig.add_subplot(111)
    plot_map(i, size=(300,300), map_origin=o_utm32_map)
    
    paths = {'fix': ('/fix', fix_path, fix_covariance, 'red'),
             'android fix': ('/android/fix',fix_path, fix_covariance, 'blue'),
             'odom': ('/pose', odometry_path, None, 'black'),
             #'gps': ('/odometry/gps', odometry_path, None)
             'filtered': ('/odometry/filtered', odometry_path, odom_covariance, 'green')
            }
    
    handles = []
    for label, (topic, p, c, color) in paths.items():
        if p:
            path = plot_path(ax, p(bag, topic), color=color, label=label)
            if path:
                handles.append(path)
        if c:
            plot_cov(ax, c(bag, topic), fc=color, ec=color)
    
    legend(handles=handles)
    fig.show()
In [26]:
bag = rosbag.Bag('../bags/2016-03-24-13-11-54.bag')
plot_paths(bag)
In [20]:
bag = rosbag.Bag('../bags/2016-03-24-13-14-55.bag')
plot_paths(bag)
In [21]:
bag = rosbag.Bag('../bags/post_2016-03-24-22-22-25.bag')
plot_paths(bag)
In [22]:
bag = rosbag.Bag('../bags/post_2016-03-24-22-22-25.bag')
In [23]:
def odometry_yaw(bag, topic):
    ms = bag.read_messages(topics=[topic])
    return np.array([(ts.to_sec(), np.arcsin(d.pose.pose.orientation.z)*2) for _, d, ts in ms])
In [24]:
def imu_yaw(bag, topic):
    ms = bag.read_messages(topics=[topic])
    return np.array([(ts.to_sec(),np.arcsin(d.orientation.z)*2) for _, d, ts in ms])

def imu_yaw_speed(bag, topic):
    ms = bag.read_messages(topics=[topic])
    return np.array([(ts.to_sec(),d.angular_velocity.z) for _, d, ts in ms])
In [25]:
t, y = odometry_yaw(bag,'/odometry/filtered').T
plot(t,y,'green')
t, y = imu_yaw(bag,'/imu').T
plot(t,y,'blue')

# t, s = imu_yaw_speed(bag,'/imu').T
# ss = np.cumsum(s)*np.gradient(t)+y[0]
# ss = ( ss + np.pi) % (2 * np.pi ) - np.pi
Out[25]:
[<matplotlib.lines.Line2D at 0x11763ded0>]